import taichi as ti
import numpy as np
import math
ti.init(arch=ti.gpu)
gui = ti.GUI("CCD", (1000, 1000))
step = 0
last_step = -1
bones = np.array([[0,0], [0.1,0.1], [0.2, 0.2], [0.3, 0.3], [0.4, 0.4], [0.5, 0.5], [0.6, 0.6]])
bone_len = len(bones)
times = 0 # 运行了多少个循环
times_step = 0 # 循环内进行到第几个骨头
target = None
# 初始化控件
ccd_btn = gui.button('begin CCD')
tolerance_sli = gui.slider("tolerance", 0.01, 0.7, 0.03)
maxIterration_sli = gui.slider('maxIterration', 1, 10)
tolerance_sli.value = 0.1
maxIterration_sli.value = 4

def process_event():
    """处理信号"""
    global step
    global target
    for e in gui.get_events():
        if e.key == 'x':
            step = step + 1
        elif e.key == 'q':
            step = 0
            target = gui.get_cursor_pos()
        elif e.key == ti.GUI.RMB:
            target = None


    

def get_angle_by_3point(p1, p2, p3):
    """求角ABC
    计算公式： cosα=A×B/∥A∥∥B∥
    方向： abx * acy - acx * aby 取正负号
    """
    a_b_x = p1[0] - p2[0];
    a_c_y = p1[1] - p3[1];
    a_c_x = p1[0] - p3[0];
    a_b_y = p1[1] - p2[1];
    c_b_x = p3[0] - p2[0];
    c_b_y = p3[1] - p2[1];
    ab_mul_cb = a_b_x * c_b_x + a_b_y * c_b_y;
    dist_ab = ti.sqrt(a_b_x * a_b_x + a_b_y * a_b_y);
    dist_cd = ti.sqrt(c_b_x * c_b_x + c_b_y * c_b_y);
    cosValue = ab_mul_cb / (dist_ab * dist_cd);
    t = a_b_x * a_c_y - a_c_x * a_b_y
    if t < 0:
        return - ti.acos(cosValue);
    return ti.acos(cosValue);


def rotate_point(central_point, src_points, angle):
    """多个点绕另一个点旋转"""
    res = []
    for src_point in src_points:
        dstx = (src_point[0]-central_point[0])*ti.cos(angle) + (src_point[1]-central_point[1])*ti.sin(angle)+central_point[0]
        dsty = (src_point[1]-central_point[1])*ti.cos(angle) - (src_point[0]-central_point[0])*ti.sin(angle)+central_point[1]
        res.append([dstx, dsty])
    return res


def run():
    global last_step
    global step
    global bones

    if not target:
        return
    if step == last_step:
        return
    times = int(step / (bone_len - 1))
    times_step = step % (bone_len - 1) + 1
    last_bone = bones[-1] # 尾节点
    if math.dist(last_bone, target) < tolerance_sli.value:
        print('达到容差值，返回')
        return
    if times > maxIterration_sli.value:
        print('超过最大迭代次数，返回')
        return
    
    last_step = step
    src_points = bones[-times_step:]
    central_p = bones[(-times_step-1)]
    angle = get_angle_by_3point(last_bone, central_p, target)
    bones[-times_step:] = rotate_point(central_p, src_points, angle)


while gui.running:
    process_event()
    run()
    if target:
        gui.circle(target, 0xFF0000, 7)
    gui.lines(bones[:-1], bones[1:], radius = 3)
    gui.circles(bones, 0xFF0011, radius = 3)
    gui.show()
